#include "myslam/stereo_undistort.h"
using namespace std;
using namespace cv;

namespace myslam {

Stereo_Undistort::Stereo_Undistort(){

    Left_Raw_K = ( Mat_<double> ( 3,3 ) << 689.8476, -0.2515,   620.8508, 
						                                        0.0,      691.1252, 489.8400,
 						                                        0.0,      0.0,      1.0 );
    
    Left_distCoeff = ( Mat_<double> ( 5,1 ) << -0.3592, 0.1235, 2.9688e-06, -0.0015, -0.0185);
    
    Right_Raw_K = ( Mat_<double> ( 3,3 ) << 680.8634, -0.1837,  655.5514, 
						                                        0.0,      682.0648, 501.6723,
						                                        0.0,      0.0,      1.0 );
    
    Right_distCoeff = ( Mat_<double> ( 5,1 ) << -0.3642, 0.1285, 7.0494e-04, 0.0012, -0.0199);
    
    Rotation_r2l = ( Mat_<double> ( 3,3 ) << 0.9997,	 -0.0020,    0.0246,
						                                         0.0020,  1.0000,     -0.0024,
			                                    			        -0.0245,  0.0025, 0.9997 );
    //const Mat Tr2l = ( Mat_<double> ( 3,1 ) << -106.2563, 0.0997, 1.7462 );
    
    Translation_r2l = ( Mat_<double> ( 3,1 ) << -1.062491e-1, 2.033e-4, 1.1014e-3);
    
    imgWidth = 1280;
    imgHeight = 1024;
	
    imageSize = Size(imgWidth, imgHeight);
    undistort_imageSize = Size(1280,1024);
    alpha = -1;
    
    stereoRectify(Left_Raw_K,Left_distCoeff,Right_Raw_K,Right_distCoeff,imageSize,
                            Rotation_r2l,Translation_r2l,Left_Undistort_Rotation,Right_Undistort_Rotation,
                            Left_New_K,Right_New_K,Q,CALIB_ZERO_DISPARITY,alpha,undistort_imageSize,&validRoiL,&validRoiR);
    initUndistortRectifyMap(Left_Raw_K,Left_distCoeff,Left_Undistort_Rotation,Left_New_K,
                                                        imageSize,CV_8UC1,rmap[0][0],rmap[0][1]);
    initUndistortRectifyMap(Right_Raw_K,Right_distCoeff,Right_Undistort_Rotation,Right_New_K,
                                                        imageSize,CV_8UC1,rmap[1][0],rmap[1][1]);
    finish_undistort_flag_ = true;
    pic_undistort_index_ = 0;
    for(int i=0;i<undistort_pic_buffer_size;i++){
        left_undistort_pic_[i] = new Mat(imgHeight,imgWidth,CV_8UC1);
        right_undistort_pic_[i] = new Mat(imgHeight,imgWidth,CV_8UC1);
    }

    stereo_undistort_running_.store(true);
    stereo_undistort_thread_ = std::thread(std::bind(&Stereo_Undistort::Stereo_Undistort_Loop, this));
}

Stereo_Undistort::~Stereo_Undistort(){
    Stereo_Undistort_Stop();
}

void Stereo_Undistort::Stereo_Undistort_Stop(){
    
    stereo_undistort_running_.store(false);
    stereo_undistort_thread_.join();

    for(int i=0;i<undistort_pic_buffer_size;i++){
        free(left_undistort_pic_[i]);
        free(right_undistort_pic_[i]);
    }
    sleep(1);
}

void Stereo_Undistort::Stereo_Undistort_Process(unsigned char* g_pRgbBuffer_left,unsigned char* g_pRgbBuffer_right){
    if(finish_undistort_flag_==true){
        std::unique_lock<std::mutex> lock(data_mutex_);
        g_pRgbBuffer_left_ = g_pRgbBuffer_left;
        g_pRgbBuffer_right_ = g_pRgbBuffer_right;
        finish_undistort_flag_ = false;
        pic_process_.notify_one();
    }
}

void Stereo_Undistort::Stereo_Undistort_Loop(){
    // boost::format fmt1("./00/image_0/%06dl.bmp");
    // boost::format fmt2("./00/image_0_undistort/%06dl_undistort.bmp");
    // boost::format fmt3("./00/image_1/%06dr.bmp");
    // boost::format fmt4("./00/image_1_undistort/%06dr_undistort.bmp"); 
    int process_index_ = 0;
    while(stereo_undistort_running_.load()){        
        std::unique_lock<std::mutex> lock(data_mutex_);
        pic_process_.wait(lock);

        // string InputPathL = (fmt1 % i).str();
        // string InputPathR = (fmt3 % i).str();
        Mat RawImageL(imgHeight,imgWidth,CV_8UC1,(void *)g_pRgbBuffer_left_);
        //imshow("Left_RawImage", RawImageL);
        
        Mat RawImageR(imgHeight,imgWidth,CV_8UC1,(void *)g_pRgbBuffer_right_);
        //imshow("Right_RawImage",RawImageR);
        
        // Mat UndistortImageL,UndistortImageR;

        remap(RawImageL, *left_undistort_pic_[pic_undistort_index_], rmap[0][0], rmap[0][1], INTER_LINEAR);
        //imshow("Left_UndistortImage", UndistortImageL);
        // string OutputPathL = (fmt2 % process_index_).str();
        // imwrite(OutputPathL, UndistortImageL);	

        remap(RawImageR, *right_undistort_pic_[pic_undistort_index_], rmap[1][0], rmap[1][1], INTER_LINEAR);
        //imshow("RIght_UndistortImage", UndistortImageR);
        // string OutputPathR = (fmt4 % process_index_).str();
        // imwrite(OutputPathR, UndistortImageR);
        
        visualodemetry_->Visual_Odometry_Update(left_undistort_pic_[pic_undistort_index_],right_undistort_pic_[pic_undistort_index_]);
        pic_undistort_index_ = (pic_undistort_index_+1)%undistort_pic_buffer_size;
        process_index_++;
        finish_undistort_flag_ = true;
    }
}

}  // namespace myslam
